/**
 * The Woosta Table Football Engine
 *
 * License... etc.
 **
 * Physics Engine - Rigid Body Dynamics
 *
 * See physics.cpp for full description.
 **
 * Author: Sean Micklethwaite
 **/

#include "physics.h"
#ifdef __PHYSICS_INTERNAL__
#define NCHECKS

using namespace WtfEngine;


/**** PhysicalSystem ****/

PhysicalSystem::PhysicalSystem()
	: GcObject()
{
	gSystems.insert(this);
};

PhysicalSystem::~PhysicalSystem()
{
	gSystems.erase(this);
};


/**
 * Computes the real resultant forces on each object, based
 * upon the combination of external forces acting upon the
 * system and internal forces at each connection point.
 * 
 * Connections may 'break' if the magnitude of the internal
 * force is greater than the connection's strength, upon which
 * the connection's OnBreak functor is called (if defined), and
 * the system is split.
 *
 * Resistive forces are also applied at connection points, which
 * are in opposite direction to the net motion at the connection
 * point.
 **/
void PhysicalSystem::ResolveSystem()
{
	tPhysicalConnectionList::iterator				iPhys;
	tStaticConnectionList::iterator					iStatic;
	int												numConstraints;
	
	//
	// Initialize each connection point
	//
	
	numConstraints = 0;
	for(iStatic = mStaticConnections.begin();
		iStatic != mStaticConnections.end(); iStatic++)
	{
		iStatic->second->iConstraint = numConstraints;
		iStatic->second->iStage = 0;
		iStatic->second->vForce = tVector();
		numConstraints++;
	};
	for(iPhys = mPhysicalConnections.begin();
		   iPhys != mPhysicalConnections.end(); iPhys++)
	{
		iPhys->second->iConstraint = numConstraints;
		iPhys->second->iStage = 0;
		iPhys->second->vForce = tVector();
		numConstraints++;
	};
	
	
	for(iStatic = mStaticConnections.begin();
		   iStatic != mStaticConnections.end(); iStatic++)
	{
		EvalStatic(iStatic->second, iStatic->first->getResultantForce(), 1, 1);
	};
	for(iPhys = mPhysicalConnections.begin();
		   iPhys != mPhysicalConnections.end(); iPhys++)
	{
		EvalPhysical(iPhys->second, iPhys->first->getResultantForce(), 1, 1);
	};
	
	for(iStatic = mStaticConnections.begin();
		   iStatic != mStaticConnections.end(); iStatic++)
	{
		iStatic->second->Apply();
	};
	
	
	//
	// Finally, check for connection breakage
	//
	
	for(iStatic = mStaticConnections.begin();
		iStatic != mStaticConnections.end();)
	{
		if(iStatic->second->vNormal.dot(iStatic->first->getVelocity()) > 0){
			// Break
			std::clog << "Connection broken" << std::endl;
			mStaticConnections.erase(iStatic++);
		} else {
#ifndef NCHECKS
			if(iStatic->second->vNormal.dot(iStatic->first->getVelocity()) < 0)
				std::cerr << "Warning: object moving into connection joint" << std::endl;
#endif
			iStatic++;
		};
	};
};

void PhysicalSystem::EvalStatic(StaticConnection *pCon, tVector vForce, int stage, int n)
{
	// Keep track of the stage of the computation, to prevent cyclic
	// dependencies
	if(stage <= pCon->iStage)
		return;
	else pCon->iStage = stage;
	
	
	//
	// Perform depth-first evaluation of the connections.
	//
	
	tPhysicalConnectionList::iterator				iPhys;
	tStaticConnectionList::iterator					iStatic;
	tVector											vExtForce;
	
	for(iStatic = mStaticConnections.find(pCon->pConnector);
		   iStatic != mStaticConnections.end(); iStatic++)
	{
		EvalStatic(iStatic->second, vForce, stage, n + 1);
		vExtForce += iStatic->second->vForce;
	};
	for(iPhys = mPhysicalConnections.find(pCon->pConnector);
		   iPhys != mPhysicalConnections.end(); iPhys++)
	{
		EvalPhysical(iPhys->second, vForce, stage, n + 1);
		vExtForce += (pCon->pConnector == iPhys->first ?
			iPhys->second->vForce : -iPhys->second->vForce);
	};
	
	//
	// Now vExtForce equals the sum of all other forces on the object.
	// Take the resultant force (minus all other forces) and split it
	// evenly among all nested connection points (by dividing by n). This
	// force is our initial approximation.
	//
	
	pCon->vForce = (vForce - vExtForce) / n;
	
	//std::clog << "EvalStatic" << std::endl;
};

void PhysicalSystem::EvalPhysical(PhysicalConnection *pCon, tVector vForce, int stage, int n)
{
	// Keep track of the stage of the computation, to prevent cyclic
	// dependencies
	if(stage <= pCon->iStage)
		return;
	else pCon->iStage = stage;
	
	
	//
	// Perform depth-first evaluation of the connections.
	//
	
	tPhysicalConnectionList::iterator				iPhys;
	tStaticConnectionList::iterator					iStatic;
	tVector											vExtForce;
	
	for(iStatic = mStaticConnections.find(pCon->pConnector);
		   iStatic != mStaticConnections.end(); iStatic++)
	{
		EvalStatic(iStatic->second, vForce, stage, n + 1);
		vExtForce += iStatic->second->vForce;
	};
	for(iPhys = mPhysicalConnections.find(pCon->pConnector);
		   iPhys != mPhysicalConnections.end(); iPhys++)
	{
		EvalPhysical(iPhys->second, vForce, stage, n + 1);
		vExtForce += (pCon->pConnector == iPhys->first ?
				iPhys->second->vForce : -iPhys->second->vForce);
	};
	
	//
	// Now vExtForce equals the sum of all other forces on the object.
	// Take the resultant force (minus all other forces) and split it
	// evenly among all nested connection points (by dividing by n). This
	// force is our initial approximation.
	//
	
	pCon->vForce = (vExtForce - vForce) / n;
};

void PhysicalSystem::StaticConnection::Apply()
{
	pConnector->applyForce(vForce, vPosition);
};

/**
 * Adds the specified connection to this system's list of connections.
 * If the connectee is already part of a physical system, the systems
 * are merged.
 **/
void PhysicalSystem::InsertConnection(PhysicalConnection* pConnection)
{
	if(pConnection->pConnectee->hasPhysicalSystem()){
		
		PhysicalSystem * pSys = pConnection->pConnectee->getPhysicalSystem();
		
		mPhysicalConnections.insert(pSys->mPhysicalConnections.begin(),
									pSys->mPhysicalConnections.end());
		mStaticConnections.insert(pSys->mStaticConnections.begin(),
							      pSys->mStaticConnections.end());
	};
	
	pConnection->pConnectee->setPhysicalSystem(this);
	
	// Insert as both connector and connectee
	mPhysicalConnections.insert(std::make_pair(pConnection->pConnector, pConnection));
	mPhysicalConnections.insert(std::make_pair(pConnection->pConnectee, pConnection));
};

void PhysicalSystem::InsertConnection(StaticConnection* pConnection)
{
	mStaticConnections.insert(std::make_pair(pConnection->pConnector, pConnection));
};

/**
 * Removes the specifed connection from the system. If the connectee
 * has other connections in this system whose paired objects also have
 * other connections in this system, the system remains intact without
 * the specified connection.
 * If the connectee has other connections in this system whose paired
 * objects have no other connections in this system, the system is split.
 * If the connectee has no other connections, the connectee becomes a
 * solitary object.
 **/
/*void PhysicalSystem::RemoveConnection(Connection* pConnection)
{

};*/

bool PhysicalSystem::hasConnection(Physical *pConnector, Static * pConnectee, const tPosition& pos)
{
	tStaticConnectionList::iterator					iStatic;
	
	for(iStatic = mStaticConnections.find(pConnector);
		   iStatic != mStaticConnections.end(); iStatic++)
	{
		if(iStatic->second->vPosition == pos)
			return true;
	};
	
	return false;
};


/**
 * Resolves all physical systems
 **/
void PhysicalSystem::Update()
{
	std::set<PhysicalSystem*>::iterator	i;
	for(i = gSystems.begin(); i != gSystems.end(); i++)
		(*i)->ResolveSystem();
};

bool PhysicalSystem::Connect(Physical* pConnector, Physical* pConnectee,
		const tVector& point, const tVector& normal, const tVector& relVel,
		tScalar strength, tScalar resistance)
{
	tDirection	impulse;
	tDirection	force;
	tDirection	planarForce;

	// The time period the force is spread over
	tScalar		softness = 0.1;
	// Coefficient of restitution
	tScalar e = 0.3;
	// Magnitude of normal reaction force
	tScalar r, friction;

	tPosition collideePoint(pConnector->getOrientation()
		* ((pConnector->getOrientation().transpose() * point)
		+ pConnector->getPosition()	- pConnectee->getPosition()));

	if(pConnector->mNumInteractions > pConnectee->mNumInteractions)
		return PhysicalSystem::Connect(pConnectee, pConnector, collideePoint, -normal, -relVel, strength, resistance);
	
	impulse = -((relVel * (1 + e))
		/ (pConnector->getMassQuot() + pConnectee->getMassQuot() + 
		( (pConnector->getInverseInertiaTensor()
			* point.cross(normal)).cross(point)
		+ (pConnectee->getInverseInertiaTensor()
			* collideePoint.cross(-normal)).cross(collideePoint)).dot(-normal)));


	// The net force exerted on this object
	force = (impulse / softness) + pConnector->getResultantForce()
		- pConnectee->getResultantForce();
	planarForce = force - normal.project(force);
	r = force.dot(normal);
	friction = abs(resistance * r);

	// Project impulse onto normal
	impulse = normal.project(impulse);

	if(pConnector->mNumInteractions < INTERACTION_THRESHOLD) {

		//
		// Connection is breaking, apply impulse, and a frictional force
		//

		pConnector->applyImpulse(impulse, point);
		pConnectee->applyImpulse(pConnector->getOrientation() * pConnectee->getOrientation().transpose() * -impulse, collideePoint);
		pConnectee->setActive();
		
		if(planarForce.length() > 0){
			planarForce = planarForce.normalize() * friction * softness;
			pConnector->applyImpulse(planarForce, point);
			pConnectee->applyImpulse(-planarForce, point);
		};

		pConnector->mNumInteractions++;
		pConnectee->mNumInteractions++;
		return false;
	}else{
		// Connection maintained - stop the object?
		std::clog << "W: Connection maintained" << std::endl;
		
		if(!pConnector->hasPhysicalSystem())
			pConnector->setPhysicalSystem(new PhysicalSystem());

		// Connect the objects
		pConnector->getPhysicalSystem()->InsertConnection(
									  new PhysicalConnection(pConnector, pConnectee, point,
										normal, strength, 1 / (resistance + 1)));
		
		return true;
	};
};

bool PhysicalSystem::Connect(Physical* pConnector, Static* pConnectee,
		const tVector& point, const tVector& normal, const tVector& relVel,
		tScalar strength, tScalar resistance)
{
	tDirection	impulse;
	tDirection	force;
	tDirection	planarForce;
	
	// Coefficient of restitution
	tScalar e = (pConnector->mNumInteractions >= INTERACTION_THRESHOLD) ? 0 : 0.5;
	// Magnitude of normal reaction force
	tScalar r, friction;
	
	// Check for existing connection
	if(pConnector->hasPhysicalSystem() && pConnector->getPhysicalSystem()->hasConnection(
	   pConnector, pConnectee, point)) return true;
	
	
	//
	// When connecting, the object may be moving with a velocity which requires
	// an impulse to correct the motion. If the magnitude of the impulse exceeds
	// the supplied strength of connection, the connection will be broken instantly,
	// and will be treated as a collision between the two objects.
	//

	impulse = ((relVel * (1 + e))
			/ (pConnector->getMassQuot() + 
			( (pConnector->getInverseInertiaTensor()
				* point.cross(normal)).cross(point)).dot(normal)));

	// Calculate magnitude of impulse
	r = impulse.dot(normal);
	
	// Project impulse onto normal
	planarForce = impulse;
	impulse = normal * r;
	planarForce = planarForce - impulse;
	
	// Calculate magnitude of frictional force
	friction = minf(planarForce.length(), abs(resistance * r));
	planarForce = planarForce.normalize() * friction;
	
#ifndef NCHECKS
	if(normal.dot(relVel) > 0){ // Pulling away
		throw std::logic_error("Collision detected, but object moving away from plane");
	};
#endif
	
	//
	// Connection is breaking by impulse, apply it, and a frictional force
	//

	pConnector->applyImpulse(-(impulse + planarForce), point);
	pConnector->applyForce(normal.project(pConnector->getResultantForce()), point);

	pConnector->mNumInteractions++;
	return (pConnector->mNumInteractions >= INTERACTION_THRESHOLD);
};

#endif
